import Leap, cv2
from supportFunctions import *
from time import sleep
import numpy as np

# 创建 Leap Motion 控制器对象
controller = Leap.Controller()
# 允许访问原始图像数据
controller.set_policy(Leap.Controller.POLICY_IMAGES)

# 等待一段时间以确保控制器初始化完成
sleep(0.1)

# 获取当前帧和左侧图像
frame = controller.frame()
image = frame.images[0]

# 获取左右图像的畸变矫正参数
left_coordinates, left_coefficients = convert_distortion_maps(frame.images[0])
right_coordinates, right_coefficients = convert_distortion_maps(frame.images[1])

# 将图像数据封装为 NumPy 数组
i_address = int(image.data_pointer)
ctype_array_def = ctypes.c_ubyte * image.height * image.width
as_ctype_array = ctype_array_def.from_address(i_address)
as_numpy_array = np.ctypeslib.as_array(as_ctype_array)
rawImage = np.reshape(as_numpy_array, (image.height, image.width))

# 创建用于合并的带有颜色的图像
mergedImage = np.zeros((image.height, image.width, 3))
mergedImage[:, :, 0] = rawImage

# 遍历手指
for finger in frame.hands[0].fingers:

    # 找到手指末端和基部的坐标
    horizontal_slope = -1 * (finger.tip_position.x - 20) / finger.tip_position.y
    vertical_slope = finger.tip_position.z / finger.tip_position.y

    # 将坐标映射到图像坐标
    pixel = image.warp(Leap.Vector(horizontal_slope, vertical_slope, 0))
    print pixel.x, pixel.y

    # 将映射的坐标转换为图像索引
    pixelIndices = [math.floor(pixel.y), math.floor(pixel.x)]

    # 在合并图像中标记手指末端点
    mergedImage[pixelIndices[0], pixelIndices[1], 1] = 255

    # 找到手指骨骼的另一端坐标
    bone = finger.bone(2).prev_joint
    horizontal_slope = -1 * (bone.x - 20) / bone.y
    vertical_slope = bone.z / bone.y

    # 将坐标映射到图像坐标
    pixel = image.warp(Leap.Vector(horizontal_slope, vertical_slope, 0))
    print pixel.x, pixel.y

    # 将映射的坐标转换为图像索引
    pixelIndices = [math.floor(pixel.y), math.floor(pixel.x)]

    # 在合并图像中标记手指骨骼的另一端点
    mergedImage[pixelIndices[0], pixelIndices[1], 1] = 255

# 使用畸变矫正参数将合并图像映射到目标图像
destination = cv2.remap(mergedImage, left_coordinates, left_coefficients, interpolation=cv2.INTER_LINEAR)
destination = cv2.resize(destination, (400, 400), 0, 0, cv2.INTER_LINEAR)

# 保存合并图像和映射后的图像
# cv2.imwrite('mergedImage.png', mergedImage)
cv2.imwrite('mergedMapped.png', destination)
# cv2.imwrite('cleanLabeledImage.png', cleanLabeledImage)
